#include "AppMain.h"
#include "usart.h"
#include "tim.h"
#include "MPU6050.h"
#include "Display.h"
#include "DisplayUI.h"
#include "SerialProtocol.h"
#include "motor_servo.h"
#include "motor_qdrive_pm4310.h"
#include "Chassis.h"

pm4310 motors[2] = {
    {&hfdcan1, 0x02},   // 左轮
    {&hfdcan1, 0x03}    // 右轮
};

Chassis chassis = {
    1000,
    {motors[0], PID{PID::PID_type::position_type, 0, 0, 0}, PID{PID::PID_type::position_type, 100, 0, 0}},
    {motors[1], PID{PID::PID_type::position_type, 0, 0, 0}, PID{PID::PID_type::position_type, 100, 0, 0}}
};

float Info_MPUyaw, Info_MPUroll, Info_MPUpinch;
char Info_Message0String[64] = {'\0'};
char Info_Message1String[64] = {'\0'};
char Info_Message2String[64] = {'\0'};
char Info_Message3String[64] = {'\0'};

uint8_t uart5RxBuffer;
SerialProtocol_HandleTypedef uart5ProtocolHandle;

uint8_t swShiftRegister = 0x00;
uint8_t swStatus = 0x00;

void CAN_Init(FDCAN_HandleTypeDef *hfdcan);


void AppMain_uart5ProtocolCallback(uint8_t addr, uint8_t length, uint8_t *data) {
    printf("[SerialRecv] Addr: %d,length %d\r\n", addr, length);
    switch (addr) {
        case 0x00: {
            chassis.speed_y = *(float *)data;
        }
        break;
        case 0x01: {
            // data单位°/s,speed_z单位rpm
            chassis.speed_z = *(float *)data / 6;
        }
        break;
        case 0x02: {
            // 最长可以接收63字节的字符串数据（必须为英文否则必报错）
            char *headPtr = Info_Message0String;
            for (int i = 0; i < length; i++) {
                *headPtr = (char)data[i];
                headPtr++;
            }
            *headPtr = '\0';
        }
        break;
        case 0x03: {
            // 最长可以接收63字节的字符串数据（必须为英文否则必报错）
            char *headPtr = Info_Message1String;
            for (int i = 0; i < length; i++) {
                *headPtr = (char)data[i];
                headPtr++;
            }
            *headPtr = '\0';
        }
        break;
        case 0x04: {
            // 最长可以接收63字节的字符串数据（必须为英文否则必报错）
            char *headPtr = Info_Message2String;
            for (int i = 0; i < length; i++) {
                *headPtr = (char)data[i];
                headPtr++;
            }
            *headPtr = '\0';
        }
        break;
        case 0x05: {
            // 最长可以接收63字节的字符串数据（必须为英文否则必报错）
            char *headPtr = Info_Message3String;
            for (int i = 0; i < length; i++) {
                *headPtr = (char)data[i];
                headPtr++;
            }
            *headPtr = '\0';
        }
        break;
        case 0x80: {
            float buffer[3] = {Info_MPUyaw, Info_MPUroll, Info_MPUpinch};
            SerialProtocol_Reply(&uart5ProtocolHandle, (uint8_t *)buffer, 12);
        }
        break;
        case 0x81: {
            SerialProtocol_Reply(&uart5ProtocolHandle, (uint8_t *)&swStatus, 1);
        }
        break;
        default: break;
    }
}

void AppMain_ActivateUART5Handler() {
    SerialProtocol_InitHandle(&uart5ProtocolHandle, 64);
    SerialProtocol_RegisterCallback(&uart5ProtocolHandle, AppMain_uart5ProtocolCallback);
    HAL_UART_Receive_IT(&huart5, &uart5RxBuffer, 1);
}

void AppMain_RenderUI() {
    DisplayUI_FillArea(0, 0, 320, 20, 0x0000);
    DisplayUI_DrawStringFmt(0, 0, &pixelFont_11x18, 0x0000, 0xffff, "MPU6050_Yaw: %0.2f", Info_MPUyaw);

    DisplayUI_FillArea(0, 20, 320, 20, 0x0000);
    DisplayUI_DrawStringFmt(0, 20, &pixelFont_11x18, 0x0000, 0xffff, "MPU6050_Roll: %0.2f", Info_MPUroll);

    DisplayUI_FillArea(0, 40, 320, 20, 0x0000);
    DisplayUI_DrawStringFmt(0, 40, &pixelFont_11x18, 0x0000, 0xffff, "MPU6050_Pinch: %0.2f", Info_MPUpinch);

    DisplayUI_FillArea(0, 80, 320, 20, 0x0000);
    DisplayUI_DrawStringFmt(0, 80, &pixelFont_11x18, 0x0000, 0xffff, "Message:");

    DisplayUI_FillArea(0, 100, 320, 10, 0x0000);
    DisplayUI_DrawString(0, 100, &pixelFont_7x10, 0x0000, 0xffff, Info_Message0String);

    DisplayUI_FillArea(0, 110, 320, 10, 0x0000);
    DisplayUI_DrawString(0, 110, &pixelFont_7x10, 0x0000, 0xffff, Info_Message1String);

    DisplayUI_FillArea(0, 120, 320, 10, 0x0000);
    DisplayUI_DrawString(0, 120, &pixelFont_7x10, 0x0000, 0xffff, Info_Message2String);

    DisplayUI_FillArea(0, 130, 320, 10, 0x0000);
    DisplayUI_DrawString(0, 130, &pixelFont_7x10, 0x0000, 0xffff, Info_Message3String);
}

void AppMain_SystemReset() {
    __set_FAULTMASK(1);
    NVIC_SystemReset();
    while (1);
}

void AppMain() {
    int ret = 0;
    // 初始化显示屏
    Display_Init();
    Display_Clear(0x0000);
    Display_SetBackLight(1);

    DisplayUI_DrawStringFmt(0, 0, &pixelFont_11x18, 0x0000, 0xffff, "=====  INIT  =====");

    // 初始化按键检测的中断
    HAL_TIM_Base_Start_IT(&htim14);

    // 初始化串口
    AppMain_ActivateUART5Handler();

    // 使能CAN
    DisplayUI_DrawStringFmt(0, 20, &pixelFont_11x18, 0x0000, 0xffff, "Initializing CAN...");
    CAN_Init(&hfdcan1);
    DisplayUI_DrawStringFmt(0, 40, &pixelFont_11x18, 0x0000, 0xffff, "CAN initialized.");

    // 重设电机ID
    // motors[0].set_id(1);while(1);

    // 初始化底盘
    HAL_TIM_Base_Start_IT(&htim16);
    chassis.initialize();
    chassis.enable();
    // chassis.speed_x = 10;
    chassis.start();
    DisplayUI_DrawStringFmt(0, 60, &pixelFont_11x18, 0x0000, 0xffff, "CAN Motor enabled.");
    HAL_Delay(1000);
    // DisplayUI_FillArea(0, 0, 320, 240, 0x0000);

    // 初始化MPU6050
    HAL_Delay(200);
    ret = MPU6050_Init();
    if (ret != 0) {
        DisplayUI_DrawStringFmt(0, 80, &pixelFont_11x18, 0x0000, 0xffff, "MPU6050_Init() failed.");
        DisplayUI_DrawStringFmt(0, 100,&pixelFont_11x18, 0x0000, 0xffff, "System resetting...");
        HAL_Delay(1000);
        AppMain_SystemReset();
    }
    ret = MPU6050_SelfTest();
    if (ret != 0) {
        DisplayUI_DrawStringFmt(0, 80, &pixelFont_11x18, 0x0000, 0xffff, "MPU6050_SelfTest() failed.");
        DisplayUI_DrawStringFmt(0, 100,&pixelFont_11x18, 0x0000, 0xffff, "System resetting...");
        HAL_Delay(1000);
        AppMain_SystemReset();
    }
    DisplayUI_DrawStringFmt(0, 80, &pixelFont_11x18, 0x0000, 0xffff, "MPU6050 initialized.");
    HAL_Delay(100);

    HAL_TIM_PWM_Start(&htim17,TIM_CHANNEL_1);
    htim17.Instance->CCR1 = 1000; // 灯的亮度为100%
    while (1) {
        AppMain_RenderUI();
        HAL_Delay(200);
    }
}

// === HAL库的回调函数 ===

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if (huart->Instance == huart5.Instance) {
        SerialProtocol_PutByte(&uart5ProtocolHandle, uart5RxBuffer);
        SerialProtocol_RequestCallback(&uart5ProtocolHandle);
        HAL_UART_Receive_IT(&huart5, &uart5RxBuffer, 1);
    }
}


void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
    if (GPIO_Pin == MPU6050_INT_Pin) {
        MPU6050_ReadAngle(&Info_MPUyaw, &Info_MPUroll, &Info_MPUpinch);
        Info_MPUyaw *= 180 / numbers::pi_v<float>;
        Info_MPUroll *= 180 / numbers::pi_v<float>;
        Info_MPUpinch *= 180 / numbers::pi_v<float>;
    } else if (GPIO_Pin == BTN0_Pin) {
        // 急停
        chassis.speed_x = 0;
        chassis.speed_y = 0;
        chassis.speed_z = 0;
    }
}


void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    if (htim->Instance == htim16.Instance) {
        for (auto motor : motors) {
            chassis.ctrl();
        }
    }
    if (htim->Instance == htim14.Instance)
    {
        // 挪用FPGA上的消抖方法
        swShiftRegister = (swShiftRegister << 1) | (HAL_GPIO_ReadPin(SW_PUSH_GPIO_Port,SW_PUSH_Pin) == GPIO_PIN_SET ? 0x01 : 0x00);
        if(swShiftRegister == 0xff) swStatus = 0x01;
        if(swShiftRegister == 0x00) swStatus = 0x00;
    }
}


void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
    if (hfdcan == &hfdcan1) {
        FDCAN_RxHeaderTypeDef rxHeader;
        uint8_t rxData[8];
        while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan,FDCAN_RX_FIFO0)) {
            if (HAL_OK == HAL_FDCAN_GetRxMessage(hfdcan,FDCAN_RX_FIFO0, &rxHeader, rxData)) {
                const int32_t motorID = static_cast<int32_t>(rxHeader.Identifier) - 0x500;
                for (auto& motor : motors) {
                    if (motorID == motor.id) {
                        motor.update(rxData);
                    }
                }
            }
        }
    }
}

void CAN_Init(FDCAN_HandleTypeDef *hfdcan) {
    FDCAN_FilterTypeDef filter;
    filter.IdType = FDCAN_STANDARD_ID;
    filter.FilterIndex = 0;
    filter.FilterType = FDCAN_FILTER_RANGE;
    filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    filter.FilterID1 = 0x500;
    filter.FilterID2 = 0x50f;

    HAL_FDCAN_ConfigFilter(hfdcan, &filter);
    HAL_FDCAN_ConfigGlobalFilter(hfdcan,FDCAN_REJECT,FDCAN_REJECT,FDCAN_FILTER_REMOTE,FDCAN_FILTER_REMOTE);
    HAL_FDCAN_ConfigFifoWatermark(hfdcan, FDCAN_CFG_RX_FIFO0, 4);
    HAL_FDCAN_ActivateNotification(hfdcan, FDCAN_IT_RX_FIFO0_WATERMARK, 0);
    HAL_FDCAN_Start(hfdcan);
}
